/**********底盘定点停车*********/
#include "global.h"
#include <string.h>
#include "bsp.h"
#include "vect.h"
#include "mymath.h"
#include "007_elmo.h"
#include "myserial.h"
#include "forward.h"
#include "climb.h"
#include "arm_radar.h"
#include "basal_move.h"
#include "Rsing_Led.h"
#include "Rsing_Can.h"
#include "ground.h"
#include "arm_radar.h"

//STR_ACC 停车后再启动加速度 （需在007_elmo.h中自行定义）
//POS_DEC 停车减速度         （需在007_elmo.h中自行定义）


  * @brief  vPosCalTask
  * @param  None
  * @retval None
  */
void vPosCalTask( void *pvParameters )
{
    float pos_x;
    float pos_y;    
    for(;;)
    {
        if(xSemaphoreTake(PosSem,60) == pdTRUE)       //等待码盘数据，最多等6ms
        {
            portENTER_CRITICAL();   
			
            /*计算陀螺仪系下的位置*/            
            pos_x=globalWHEEL_PERIMETER * ((float)xGlobalParam.original_pos_x / 2000.0f);
            pos_y=globalWHEEL_PERIMETER * ((float)xGlobalParam.original_pos_y / 2000.0f);

			 /*计算修正后位置*/
			xGlobalParam.cur_pos.x   =   pos_x*xGlobalParam.pos_off.angcosf
                                       - pos_y*xGlobalParam.pos_off.angsinf
								       + xGlobalParam.pos_off.x;
		    
			xGlobalParam.cur_pos.y   =   pos_x*xGlobalParam.pos_off.angsinf
                                       + pos_y*xGlobalParam.pos_off.angcosf
								       + xGlobalParam.pos_off.y;
            
            xGlobalParam.cur_pos.ang =   xGlobalParam.original_pos_a 
                                       + xGlobalParam.pos_off.ang;          
            
            xGlobalParam.cur_omiga=xGlobalParam.original_omiga;

            //当前速度方向向量
			set_vect(&xGlobalParam.spd_vect, &xGlobalParam.pos_old,  &xGlobalParam.cur_pos);
			//记录里程
			//xGlobalParam.mileage += get_vect_len(&xGlobalParam.spd_vect);
		    /*计算当前速度*/
		    xGlobalParam.cur_speed = (float)get_vect_len(&xGlobalParam.spd_vect)  / (float)(0.005f);
			/*更细坐标数据*/
		    memcpy(&xGlobalParam.pos_old, &xGlobalParam.cur_pos, sizeof(point_t)); 	
                
            portEXIT_CRITICAL();
        }
        else
        {
            vErrorHandle(0);
			Buzzer(10,500,100);   //陀螺仪错误，蜂鸣器叫2s
            LEDOn(LEDR);
            LEDOff(LEDG);
            LEDOff(LEDB);
			LCD_QueuePrintfErr(3,0,16, "GYRO ERR");
            vTaskSuspend(NULL);
        }
    }
}


/*
********************************************************************************
  *@  name      : Elmo_SetAcc(u8 elmoID, u32 acc, u32 dec)
  *@  function  : 设置速度模式和位置模式的电机加减速
  *@  input     : elmoID     取elmo的节点ID
  *@                 acc        加速度,加速度最大不能超过1000000000,同时应考虑电机性能
  *@                 dec        减速度,减速度最大不能超过1000000000,同时应考虑电机性能
  *@  output    : 0         函数调用成功
  *@              1         函数调用失败
********************************************************************************
*/
u8 Elmo_SetAcc(u8 elmoID, u32 acc, u32 dec)
{ 
	RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"MO", 0, TYPE_INTEGER, MO_OFF);
	Elmo_Delay100us(30);	
	RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"PM", 0, TYPE_INTEGER, 0x01);
	Elmo_Delay100us(10);	
	RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"AC", 0, TYPE_INTEGER, acc);
	Elmo_Delay100us(10);
	RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"DC", 0, TYPE_INTEGER, dec);
	Elmo_Delay100us(10);
	RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"MO", 0, TYPE_INTEGER, MO_ON);
	Elmo_Delay100us(100);
	return 0;
}


/*
********************************************************************************
  *@  name      : Elmo_RunPPM
  *@  function  : 单轴平滑速度的位置模式
  *@  input     : elmoID    取elmo节点ID
  *@              speed     速度(cnt/s)
  *@              position  目标位置(cnt)
  *@              PPMmode   运行模式
  *@				    POS_ABS // PPM运行方式:绝对位置
  *@				    POS_REL // PPM运行方式:相对位置
  *@  output    : 0         函数调用成功
  *@              1         函数调用失败
********************************************************************************
*/
u8 Elmo_PPM(u8 elmoID, u32 speed, s32 position, u8 PPMmode)
{
	u8 i = 0;

	/************如果使用广播控制,当前模式不为位置模式,切换为位置模式 *************/
	if(elmoID == 0 && elmo[elmoID].CurOPMode != UM_PCM)
	{
		if(elmo[elmoID].CurOPMode != UM_IDLE)
		{
			RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"MO", 0, TYPE_INTEGER, MO_OFF);
			Elmo_Delay100us(90);
		}	
		/* 将所有状态变量赋值为PCM */		
		elmogroup.CurOPMode = UM_PCM;			
		for(i=0;i<=ELMO_NUM;i++)
		{
			elmo[i].CurOPMode = UM_PCM;			
		}

		RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"UM", 0, TYPE_INTEGER, UM_PCM);
		Elmo_Delay100us(20);
		
		RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"MO", 0, TYPE_INTEGER, MO_ON);
		Elmo_Delay100us(30);
	}  
	/* 使用单个ELMO控制,当前模式不为位置模式,切换为位置模式 */
	else if(elmo[elmoID].CurOPMode != UM_PCM)
	{
		if(elmo[elmoID].CurOPMode != UM_IDLE)
		{
			RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"MO", 0, TYPE_INTEGER, MO_OFF);
			Elmo_Delay100us(90);
		}

		elmo[elmoID].CurOPMode = UM_PCM;
		RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"UM", 0, TYPE_INTEGER, UM_PCM);
		Elmo_Delay100us(200);

		RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"MO", 0, TYPE_INTEGER, MO_ON);
		Elmo_Delay100us(30);


        /* 如果ELMO只挂载了1个，不管怎么改其的模式，elmo[0]的模式和它一样 */				
		if(ELMO_NUM == 1)      
		{
			elmo[0].CurOPMode = UM_PCM;
		}		
		/* 用来判断是否所有电机模式相同，从而确定elmo[0]的模式 */
		else 
		{
			for(i=1;i<ELMO_NUM;i++) 
			{
				if( elmo[i].CurOPMode == elmo[i+1].CurOPMode )
				{
					elmo[0].CurOPMode = UM_PCM;
				}
				else
				{
					elmo[0].CurOPMode = UM_UNC;
					break;
				}
			}			
		}
	}

	/* 设置目标速度 */
	RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"SP", 0, TYPE_INTEGER, speed);
	Elmo_Delay100us(20);

	/* 根据运行模式设置位置 */
	if(PPMmode)
	{
		RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"PA", 0, TYPE_INTEGER, position);
		Elmo_Delay100us(20);
	}
	else
	{
		RPDO2_Cmd_data(&elmo[elmoID], (u8 *)"PR", 0, TYPE_INTEGER, position);
		Elmo_Delay100us(20);
	}
	
	/* 运行位置模式 */
	RPDO2_Cmd_string(&elmo[elmoID], (u8 *)"BG");
	Elmo_Delay100us(10);
	return 0;
}



void vPoint_Stoptask(void *pvParameters)
{
  while(1)
  {
	float distance_stop;
	distance_stop = xGlobalParam.point_pos.x - xGlobalParam.cur_pos.x ; 
	if(distance_flag <= Dec_distance)
	{
		Elmo_SetAcc(0, STR_ACC, POS_DEC);
		Elmo_PPM(0, car_speed, distance_stop, POS_REL);
	}
   }
}












